#ifndef __LQR_H__
#define __LQR_H__

#include "controller/controller_base.h"
#include <Eigen/Dense>

class LQR: public ControllerBase{
    public:
        LQR();
        ~LQR();

        // 计算局部目标点的速度
        // mc::status computeTwist(geometry_msgs::PoseStamped goal, geometry_msgs::Twist curr_vel, geometry_msgs::Twist& cmd_vel);
        mc::status computeTwist(mc::TrajectoryPoint2D goal, geometry_msgs::Twist curr_vel,
            geometry_msgs::Twist& cmd_vel);

        // LQR 求解计算
        Eigen::Vector2d LQR_Slove(Eigen::Vector3d s, Eigen::Vector3d s_d, Eigen::Vector2d u_r);

    private:
        double dt_;         //控制周期
        int max_iter_;      //最大迭代次数
        double eps_iter_;   //迭代阈值
        Eigen::Matrix3d Q_;  // state error matrix
        Eigen::Matrix2d R_;  // control error matrix        
};

#endif